   
 keyestudio Mini Tank Robot v2.0
 lesson 15
 multiple functions
 httpwww.keyestudio.com


飬ڴͼݣԼҲԴȡеõ
unsigned char start01[] = {0x01,0x02,0x04,0x08,0x10,0x20,0x40,0x80,0x80,0x40,0x20,0x10,0x08,0x04,0x02,0x01};
unsigned char front[] = {0x00,0x00,0x00,0x00,0x00,0x24,0x12,0x09,0x12,0x24,0x00,0x00,0x00,0x00,0x00,0x00};
unsigned char back[] = {0x00,0x00,0x00,0x00,0x00,0x24,0x48,0x90,0x48,0x24,0x00,0x00,0x00,0x00,0x00,0x00};
unsigned char left[] = {0x00,0x00,0x00,0x00,0x00,0x00,0x44,0x28,0x10,0x44,0x28,0x10,0x44,0x28,0x10,0x00};
unsigned char right[] = {0x00,0x10,0x28,0x44,0x10,0x28,0x44,0x10,0x28,0x44,0x00,0x00,0x00,0x00,0x00,0x00};
unsigned char STOP01[] = {0x2E,0x2A,0x3A,0x00,0x02,0x3E,0x02,0x00,0x3E,0x22,0x3E,0x00,0x3E,0x0A,0x0E,0x00};
unsigned char clear[] = {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
#define SCL_Pin  A5  ʱΪ A5
#define SDA_Pin  A4  Ϊ A4

#define ML_Ctrl 13  ߵ
#define ML_PWM 11   ߵPWM
#define MR_Ctrl 12  ұߵ
#define MR_PWM 3   ұߵPWM

#define Trig 5  ultrasonic trig Pin
#define Echo 4  ultrasonic echo Pin
int distance; 泬⵽ľֵڸ湦
int random2; ı
泬⵽ľֵڱϹ
int a;  
int a1;
int a2;

#define servoPin 9  servo Pin
int pulsewidth;

#define light_L_Pin A1   
#define light_R_Pin A2   ҹ
int left_light;
int right_light;

char bluetooth_val; ڴյֵ
int flag;  ־ڽ˳
void setup(){
  Serial.begin(9600);
  pinMode(Trig, OUTPUT);
  pinMode(Echo, INPUT);
  pinMode(ML_Ctrl, OUTPUT);
  pinMode(ML_PWM, OUTPUT);
  pinMode(MR_Ctrl, OUTPUT);
  pinMode(MR_PWM, OUTPUT);
  
  pinMode(servoPin, OUTPUT);
  procedure(90); öĽǶΪ90
 pinMode(SCL_Pin,OUTPUT);
  pinMode(SDA_Pin,OUTPUT);
  matrix_display(clear);    
  matrix_display(start01);  ʾʼͼ
  pinMode(light_L_Pin, INPUT);
  pinMode(light_R_Pin, INPUT);
}

void loop(){
  if (Serial.available())
  {
    bluetooth_val = Serial.read();
    Serial.println(bluetooth_val);
  }
  switch (bluetooth_val) 
  {
    case 'F'  ǰ
      Car_front();
      matrix_display(front);  ʾǰͼ
      break;
    case 'B'  ˺
      Car_back();
      matrix_display(back);  ʾͼ
      break;
    case 'L'  ת
      Car_left();
      matrix_display(left);  ʾתͼ
      break;
    case 'R'  ת
      Car_right();
      matrix_display(right);  ʾתͼ
      break;
    case 'S'  ֹͣ
      Car_Stop();
      matrix_display(STOP01);  ʾֹͣͼ
      break;
   case 'Y'
      matrix_display(start01);  ʾʼͼ
      follow();
      break;
   case 'U'
      matrix_display(start01);  ʾʼͼ
      avoid();
      break;
   case 'X'
      matrix_display(start01);  ʾʼͼ
      light_track();
      break;
  }}
Ϲ
void avoid() 
{
  flag = 0;  ΪϹܵı־
  while (flag == 0) 
  {
    random2 = random(1, 100);
    a = checkdistance();  ѳ⵽ǰľ븳 a
    
    if (a  20) ⵽ǰľС20cmʱ
    {
      Car_Stop();  ֹͣ
      delay(200); ʱ200ms
      
      procedure(160);  ̨ת
      for (int j = 1; j = 10; j = j + (1)) { for䣬ó⼸Σݻ׼ȷ
        a1 = checkdistance();  ѳ⵽ߵľ븳 a1
      }
      delay(200);
      procedure(20); ̨ת
      for (int k = 1; k = 10; k = k + (1)) {
        a2 = checkdistance(); ѳ⵽ұߵľ븳 a2
      }
      if (a1  50  a2  50)  ߵľһ50cmʱһת
      {
        if (a1  a2) ߵľұʱ
        {
          procedure(90);  ̨תǰ
          Car_left();  ת
          delay(500);  ת500ms
          Car_front(); ǰ
        } 
        else 
        {
          procedure(90);
          Car_right(); ת
          delay(500);
          Car_front();  ǰ
        }
      } 
      else  ⵽ߵľ붼=50cmģתת
      {
        if ((long) (random2) % (long) (2) == 0)  Ϊżʱ
        {
          procedure(90);
          Car_left(); ת
          delay(500);
          Car_front(); ǰ
        } 
        else 
        {
          procedure(90);
          Car_right(); ת
          delay(500);
          Car_front(); ǰ
       } } } 
  else  ⵽ǰľ=20cmʱǰ
  {
      Car_front(); ǰ
  }
   ٽֵΪ˳Ϲѭ
  if (Serial.available())
  {
    bluetooth_val = Serial.read();
    if (bluetooth_val == 'S')  յS
    {
      flag = 1;  flagֵΪ1,˳ѭ
    }}}}
湦
void follow() {
  flag = 0;
  while (flag == 0) {
    distance = checkdistance();  ⵽ľ븳 distance
    if (distance = 20 && distance = 60) ǰķΧ
    {
      Car_front();
    }
    else if (distance  10 && distance  20)  ֹͣķΧ
    {
      Car_Stop();
    }
    else if (distance = 10)  ˵ķΧ
    {
      Car_back();
    }
    else  ֹͣ
    {
      Car_Stop();
    }
    if (Serial.available())
    {
      bluetooth_val = Serial.read();
      if (bluetooth_val == 'S') 
      {
        flag = 1;  ˳ѭ
      }}}}
Ƴĺ
float checkdistance() {
  digitalWrite(Trig, LOW);
  delayMicroseconds(2);
  digitalWrite(Trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(Trig, LOW);
  float distance = pulseIn(Echo, HIGH)  58.00;  58.20 229.1=58.2
  delay(10);
  return distance;
}
ƶĺ
void procedure(int myangle) {
  for (int i = 0; i = 50; i = i + (1)) {
    pulsewidth = myangle  11 + 500;
    digitalWrite(servoPin,HIGH);
    delayMicroseconds(pulsewidth);
    digitalWrite(servoPin,LOW);
    delay((20 - pulsewidth  1000));
  }}

Ѱ⹦
void light_track() {
  flag = 0;
  while (flag == 0) {
    left_light = analogRead(light_L_Pin);
    right_light = analogRead(light_R_Pin);
    if (left_light  650 && right_light  650) ⵽ķΧֵǰ
    {  
      Car_front();
    } 
    else if (left_light  650 && right_light = 650)  ⵽ķΧֵת
    {
      Car_left();
    } 
    else if (left_light = 650 && right_light  650) ⵽ķΧֵת
    {
      Car_right();
    } 
    else  ֹͣ
    {
      Car_Stop();
    }
    if (Serial.available())
    {
      bluetooth_val = Serial.read();
      if (bluetooth_val == 'S') {
        flag = 1;
     }}}}
ĺ
ڵʾ
void matrix_display(unsigned char matrix_value[])
{
  IIC_start();
  IIC_send(0xc0);  ѡַ
  
  for(int i = 0;i  16;i++) ͼ16ֽ
  {
     IIC_send(matrix_value[i]); ͼ
  }
  IIC_end();   ͼݴ
  IIC_start();
  IIC_send(0x8A);  ʾƣѡΪ416
  IIC_end();
}
ݿʼ
void IIC_start()
{
  digitalWrite(SCL_Pin,HIGH);
  delayMicroseconds(3);
  digitalWrite(SDA_Pin,HIGH);
  delayMicroseconds(3);
  digitalWrite(SDA_Pin,LOW);
  delayMicroseconds(3);
}

void IIC_send(unsigned char send_data)
{
  for(char i = 0;i  8;i++)  ÿֽ8λ
  {
      digitalWrite(SCL_Pin,LOW);  ʱSCL_PinͣſԸıSDAź
      delayMicroseconds(3);
      if(send_data & 0x01)  ֽڵÿһλ10SDA_Pinĸߵ͵ƽ
      {
        digitalWrite(SDA_Pin,HIGH);
      }
      else
      {
        digitalWrite(SDA_Pin,LOW);
      }
      delayMicroseconds(3);
      digitalWrite(SCL_Pin,HIGH); ʱSCL_PinߣֹͣݵĴ
      delayMicroseconds(3);
      send_data = send_data  1;  һλһλļ⣬Խһλ
  }}
ݴı־
void IIC_end()
{
  digitalWrite(SCL_Pin,LOW);
  delayMicroseconds(3);
  digitalWrite(SDA_Pin,LOW);
  delayMicroseconds(3);
  digitalWrite(SCL_Pin,HIGH);
  delayMicroseconds(3);
  digitalWrite(SDA_Pin,HIGH);
  delayMicroseconds(3);
}
ת
void Car_front()
{
  digitalWrite(MR_Ctrl,LOW);
  analogWrite(MR_PWM,200);
  digitalWrite(ML_Ctrl,LOW);
  analogWrite(ML_PWM,200);
}
void Car_back()
{
  digitalWrite(MR_Ctrl,HIGH);
  analogWrite(MR_PWM,200);
  digitalWrite(ML_Ctrl,HIGH);
  analogWrite(ML_PWM,200);
}
void Car_left()
{
  digitalWrite(MR_Ctrl,LOW);
  analogWrite(MR_PWM,255);
  digitalWrite(ML_Ctrl,HIGH);
  analogWrite(ML_PWM,255);
}
void Car_right()
{
  digitalWrite(MR_Ctrl,HIGH);
  analogWrite(MR_PWM,255);
  digitalWrite(ML_Ctrl,LOW);
  analogWrite(ML_PWM,255);
}
void Car_Stop()
{
  digitalWrite(MR_Ctrl,LOW);
  analogWrite(MR_PWM,0);
  digitalWrite(ML_Ctrl,LOW);
  analogWrite(ML_PWM,0);
}
void Car_T_left()
{
  digitalWrite(MR_Ctrl,LOW);
  analogWrite(MR_PWM,255);
  digitalWrite(ML_Ctrl,LOW);
  analogWrite(ML_PWM,180);
}
void Car_T_right()
{
  digitalWrite(MR_Ctrl,LOW);
  analogWrite(MR_PWM,180);
  digitalWrite(ML_Ctrl,LOW);
  analogWrite(ML_PWM,255);
}
  